#include "motor_driver.h"
#include "current_ctl_loop.h"
#include "arm_math.h"
#include "adc.h" 
#include "Encoder.h"
#include "current_ctl_pi.h"
#include "debug.h"
//#include "VF.h"
//ExtY rtY;
//转换因子用户根据硬件定义
#define CURRENT_SAMP_ADC_K   0.00402832f    //         0.003597f  //(0.0067138671875F)   // 电流采样 ADC 转化系数  12bit ADC 采样电阻  0.01Ω  放大系数 10倍   3.3/4096/(10*0.01)  3.3/4096/（10*0.01）
#define pwm_FRQ (20000)     //PWM 频率 20KHz
#define PWM_ARR_HALF (2125) //PWM 占空比 50%  4250/2
#define PWM_K (177.0833f)   //PWM 占空比 和 实际电压转化系数  母线电压  24V  对应  PWM 占空比 4250  PWM_K = 4250 /  24V 

uint32_t kCurrentSampAdcMean[3] = {1985,1985,1985} ; //{1495, 1495, 1495};  //ADC中心偏移值  可调可校准  在 PWM 使能状态 相间电压实际都为0V时候，校准该值
uint32_t kEncoderRes = 131072  ;  //编码器分辨率 一圈多少脉冲
float kEncoderResInv =   7.629394e-06 ;//编码器分辨率倒数    1.52587e-05f 
//float kMotorPn = 10.0f;  //电机极对数
uint16_t IA_Offset,IB_Offset,IC_Offset;


real32_T current_ctl_pi_ud = 0.0f;
real32_T current_ctl_pi_uq = 0.0f;

extern real32_T rtb_Saturation;
extern real32_T rtb_Saturation1;

MotorDriver kMotorDriver;
//用户接口注册函数接口
//初始化
// 新增变量定义



void MotorDriverInit(void)
{
    kMotorDriver.elec_angle_bias_cal = 0;  //校准完成 为 1
    kMotorDriver.elec_angle_bias = 0.0f;    
     current_ctl_loop_initialize();
    _rt_U.i_noise_A = 0.0134f; //实测得到   0.0134f
    _rt_U.comp_du_V = 0.018f;   //实测得到   0.08f
	//初始化idq
	kMotorDriver.udq_tar_V[0] = 0.0f;
	kMotorDriver.udq_tar_V[1] = 0.0f;
//	kMotorDriver.udq_now_V[0] = 0.0f;
//	kMotorDriver.udq_now_V[1] = 0.0f;
	
}

void MotorDriver_SetIdqTar(float id, float iq){

		_rt_U.idq_tar_A[0] = id;
		_rt_U.idq_tar_A[1] = iq;


}

void MotorDriver_UodateUdqTar(void){
		current_ctl_pi_ud = rtb_Saturation;
		current_ctl_pi_uq = rtb_Saturation1;
	
		kMotorDriver.udq_tar_V[0] = current_ctl_pi_ud;
		kMotorDriver.udq_tar_V[1] = current_ctl_pi_uq;
	


}

//获取d轴电压
float MotorDriver_GetUd(void){
		return kMotorDriver.udq_tar_V[0];
}

//获取d轴电压
float MotorDriver_GetUq(void){
		return kMotorDriver.udq_tar_V[1];
}


//// 派克变换：三相电压 -> dq轴电压
//void MotorDriverParkTransform(void)
//{
//    float theta = _rt_U.elec_theta_rad; // 获取当前电角度
//    float cos_theta = cosf(theta);
//    float sin_theta = sinf(theta);
//    float cos_theta_120 = cosf(theta - 2.09439516f); // 120度 = 2π/3
//    float sin_theta_120 = sinf(theta - 2.09439516f);
//    float cos_theta_240 = cosf(theta + 2.09439516f); // 240度 = 4π/3
//    float sin_theta_240 = sinf(theta + 2.09439516f);
//    
//    // 派克变换公式
//    kMotorDriver.udq_now_V[0] = (2.0f/3.0f) * (kMotorDriver.uabc_tar_com_V[0] * cos_theta + 
//                                              kMotorDriver.uabc_tar_com_V[1] * cos_theta_120 + 
//                                              kMotorDriver.uabc_tar_com_V[2] * cos_theta_240);
//                                              
//    kMotorDriver.udq_now_V[1] = (2.0f/3.0f) * (-kMotorDriver.uabc_tar_com_V[0] * sin_theta - 
//                                               kMotorDriver.uabc_tar_com_V[1] * sin_theta_120 - 
//                                               kMotorDriver.uabc_tar_com_V[2] * sin_theta_240);
//}

//// 获取d轴当前电压
//float MotorDriver_GetUdNow(void)
//{
//    return kMotorDriver.udq_now_V[0];
//}

//// 获取q轴当前电压
//float MotorDriver_GetUqNow(void)
//{
//    return kMotorDriver.udq_now_V[1];
//}


//母线电压更新
void MotorDriverVbusUpdata(void)
{
     kMotorDriver.vbus = 24.0f;
}

//三相电流更新
void MotorDriverIabcUpdata(void)
{
    kMotorDriver.iabc_now_A[0] = (float)(int32_t)(hadc1.Instance->JDR1 - kCurrentSampAdcMean[0]) * CURRENT_SAMP_ADC_K;//U
	  kMotorDriver.iabc_now_A[2] = (float)(int32_t)(hadc2.Instance->JDR1 - kCurrentSampAdcMean[1]) * CURRENT_SAMP_ADC_K;//W
	  kMotorDriver.iabc_now_A[1] = - kMotorDriver.iabc_now_A[0] - kMotorDriver.iabc_now_A[2];
}
//编码器更新
void MotorDriverEncoderUpdata(void)
{
    kMotorDriver.encoder_cnt =   Encoder.OrigMecAngle17b;//Encoder.InOriPosition

}
//霍尔状态更新
void MotorDriverHallStateUpdata(void)
{
    //hall_state   TODO
}

//三相电压输出
void MotorDriverUabcOutput(void)
{
		uint32_t uabc_tar[3] = {0};

        for (uint8_t i = 0; i < 3; i++)
        {
            uabc_tar[i] = kMotorDriver.uabc_tar_com_V[i] * PWM_K + PWM_ARR_HALF;
            if (uabc_tar[i] > htim1.Instance->ARR)
            {
                uabc_tar[i] = htim1.Instance->ARR;
            }
        }

		htim1.Instance->CCR1 = uabc_tar[0];   //更新目标电压
		htim1.Instance->CCR2 = uabc_tar[1];
		htim1.Instance->CCR3 = uabc_tar[2]; 

}



uint16_t adc1_in1, adc1_in2, adc1_in3, adc1_in4, adc_vbus;

uint8_t ADC_offset = 0;
float Ia,Ib,Ic,Ia1,Ib1,Ic1;
/**
  * @brief  Injected conversion complete callback in non-blocking mode.
  * @param hadc ADC handle
  * @retval None
  */
	ADC_OffsetValues offsets;
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *hadc)
{
    volatile static uint8_t adc1_jeoc_flag = 0;
    volatile static uint8_t adc2_jeoc_flag = 0;
    static uint32_t elec_angle_bias_cal_cnt = 0;


	   if(hadc->Instance == ADC1) 
    {
         adc1_jeoc_flag++;
    }

    if(hadc->Instance == ADC2) 
    {
         adc2_jeoc_flag++;
    }

	if (adc1_jeoc_flag >= 1  && adc2_jeoc_flag >= 1)
    {
			  if (adc1_jeoc_flag >= 2 || adc2_jeoc_flag >= 2)
				{
		            /* code */ //中断执行错误
            __NOP();
            __NOP();
            __NOP();
				}
		    adc1_jeoc_flag = 0;
        adc2_jeoc_flag = 0;

			MotorDriverVbusUpdata();
			MotorDriverIabcUpdata();
			MotorDriverEncoderUpdata();
//			_rt_U.dc_bus_now_V = 2;
      //FOC step
			_rt_U.iabc_now_A[0] = kMotorDriver.iabc_now_A[0];
      _rt_U.iabc_now_A[1] = kMotorDriver.iabc_now_A[1];
      _rt_U.iabc_now_A[2] = kMotorDriver.iabc_now_A[2];

			 if (kMotorDriver.elec_angle_bias_cal == 0)
        {
					MotorDriver_SetIdqTar(0.5f,0.0f);					
//            _rt_U.idq_tar_A[0] = 0.5f;
//            _rt_U.idq_tar_A[1] = 0.0f;
            _rt_U.elec_theta_rad = 0.0f;  
              //持续2s
            if (elec_angle_bias_cal_cnt++ >= 131072)
            {
                  //计算电角度偏移值
                kMotorDriver.elec_angle_bias = (float)(kMotorDriver.encoder_cnt) * kEncoderResInv * kMotorPn * 2.0f * PI;

							
							
    				_rt_U.idq_tar_A[0] = 0.0f;
            _rt_U.idq_tar_A[1] = 0.0f;
                kMotorDriver.elec_angle_bias_cal = 1; //校准完成
            }
        }
        else
        {
            _rt_U.elec_theta_rad = ((float)(kMotorDriver.encoder_cnt) * kEncoderResInv * kMotorPn * 2.0f * PI - kMotorDriver.elec_angle_bias);
//					float raw_angle = (float)kMotorDriver.encoder_cnt * kEncoderResInv * kMotorPn * TWO_PI - kMotorDriver.elec_angle_bias;
//									raw_angle = fmodf(raw_angle , TWO_PI);
//									if(raw_angle < 0)raw_angle += TWO_PI;
//									_rt_U.elec_theta_rad = raw_angle;
        }

						current_ctl_loop_step1();  //电流环运行步骤
//						UpdateElectricalAngularVelocity();
						MotorDriver_UodateUdqTar();
						kMotorDriver.uabc_tar_com_V[0] = _rt_Y.uabc_tar_comp_V[0];
						kMotorDriver.uabc_tar_com_V[1] = _rt_Y.uabc_tar_comp_V[1];
						kMotorDriver.uabc_tar_com_V[2] = _rt_Y.uabc_tar_comp_V[2];
//				MotorDriverParkTransform();
//				    /* 新增：三相电压计算 ★ */
//    const real32_T Vm = 2.0F;  // U相峰值电压
//    const real32_T deg120 = 2.09439516F;  // 120°弧度值
//    const real32_T deg240 = 4.18879032F;  // 240°弧度值
//      real32_T theta = _rt_U.elec_theta_rad; // 使用设定的电角度
//    kMotorDriver.uabc_tar_com_V[0] = Vm * sinf(theta);         // U相
//    kMotorDriver.uabc_tar_com_V[1] = Vm * sinf(theta - deg120); // V相
//    kMotorDriver.uabc_tar_com_V[2]  = Vm * sinf(theta - deg240); // W相
        
       MotorDriverUabcOutput();
			

  }
}



//void TIMx_UP_M1_IRQHandler(void)
//{
// /* USER CODE BEGIN TIMx_UP_M1_IRQn 0 */
// static uint16_t DecodeEncoder;
////    dwt_measure_start(&dwt_start.TIM1Task,&dwt_timer.TIM1Task);
// /* USER CODE END  TIMx_UP_M1_IRQn 0 */

//    LL_TIM_ClearFlag_UPDATE(TIM1);
//    R3_2_TIMx_UP_IRQHandler(&PWM_Handle_M1);

// /* USER CODE BEGIN TIMx_UP_M1_IRQn 1 */
//// #define ENCODER_REDUCE_TIMES
//		debug.EncoderTxCnt++;
//    encoder_tx();
//#if defined(ENCODER_REDUCE_TIMES)
//    DecodeEncoder ^= 1;
//    if (DecodeEncoder)
//    {
//      encoder_rx_noprocess();
//    }
//    else
//    {
//#endif // ENCODER_REDUCE_TIMES
//      encoder_rx();
//#if defined(ENCODER_REDUCE_TIMES)
//    }
//#endif // ENCODER_REDUCE_TIMES
//    if (MC_Interface.StartEncCount <= 2)
//    {
//      MC_Interface.StartEncCount++;
//    }
//    timcnt++;
////    dwt_measure_end(&dwt_start.TIM1Task,&dwt_timer.TIM1Task);
// /* USER CODE END  TIMx_UP_M1_IRQn 1 */
//}
debugget_t debugget={
	.TIM01_max =20000,
};

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  if (htim->Instance == TIM1)
  {
		debugget.TIM01++;
		if(debugget.TIM01>debugget.TIM01_max){
			debugget.encoder_txcnt_max = debugget.encoder_txcnt;
			debugget.encoder_rxcnt_max = debugget.encoder_rxcnt;
				debugget.TIM01 = 0;
			debugget.encoder_txcnt = 0;
			debugget.encoder_rxcnt = 0;
			
			debugget.error=debugget.encoder_txcnt_max-debugget.encoder_rxcnt_max ;
		}
	
    // 定时器1更新中断处理逻辑
    encoder_tx(); 
		debugget.encoder_txcnt++;
    encoder_rx(); 
		
		static int ms = 0;
		if(ms++>9){
			ms = 0;
				Print_Wave(0);
		}
    
  }
	else  if(htim->Instance == TIM6){
		//
		
		
	}
	else	if(htim->Instance == TIM8){
		//
		
		
	}
}